#include "os_mem.h"
#include "os_io.h"

extern uint32_t __usr__mem__base__;
extern uint32_t __usr__mem__len__;

	
/*
* Initialize a memory region, for variable sized block requests.
* The region shall initially have one single large block.
* Requests for smaller blocks shall be serviced by splitting large blocks.
* Request for large blocks shall be serviced by merging contiguous small free blocks
*
* Memory shall be organized in the following way
*
*  ____________________________________________
* | ----------  mem_region_t -----------------|
* | __________________________________________|
* |                                           | |
* |                                           | |
* | ------- buff_hdr (len = header_len) ----- | |
* |  (header len should include mem_buffer_t) |
* |                                           | |
* |-------------------------------------------| 
* |                                           | 
* |                                           | 
* |                                           | 
* |                                           | 
* |                                           | 
* |                                           | 
* |      ----- buffer contents -----          |  
* |                                           | 
* |                                           | 
* |                                           | 
* |                                           | 
* |                                           | 
* |___________________________________________| |
*
*/
bool mem_region_init(uint8_t *start_ptr, uint32_t total_region_len, uint16_t header_len)
{
	MEM_DEBUG_PRINT("Mem region init with start_ptr:0x%x, len:0x%x, header_len:0x%x\n", start_ptr, len, header_len);

	total_region_len = total_region_len & 0xFFFFFFFC;

	if (header_len & 0x03) {
		printf("Header len should be 4 byte aligned\n");
		return FALSE;
	}


	if (((uint32_t)start_ptr & 0x03) || total_region_len < (header_len + sizeof(mem_region_t)) || header_len < sizeof(mem_buffer_t))
	{
		printf("Invalid parameters in mem_init: ptr(0x%x), len(0x%x), header_len(0x%x)\n", (uint32_t)start_ptr, total_region_len, header_len);
		return FALSE;
	}

	mem_region_t *region_ptr = (mem_region_t *)start_ptr;
	SET_MAGIC_NUM(region_ptr->magic_num[0]);
	region_ptr->region_len = (total_region_len - sizeof(mem_region_t));
	region_ptr->buffer_header_len = header_len;

	mem_buffer_t *ptr = (mem_buffer_t *)((uint8_t *)region_ptr + sizeof(mem_region_t));

#if 0
	// setup a dummy buffer which is always allocated.
	// so that prev_buffer_len of all buffers is always valid
	ptr->magic_start[0] = MEM_MAGIC_NUM;
	ptr->prev_buffer_len = 0;
	ptr->buffer_len = 0;		//(len - (header_len + sizeof(mem_region_t)));
	ptr->buffer_status = MEM_STATUS_OCCUPIED;

	ptr++;
#endif

	SET_MAGIC_NUM(ptr->magic_start[0]);
	//ptr->buffer_len = len - (header_len + header_len + sizeof(mem_region_t));
	ptr->buffer_len = total_region_len - (header_len + sizeof(mem_region_t));
	ptr->buffer_status = MEM_STATUS_FREE;

	return TRUE;
}


void user_mem_init(void)
{

	uint32_t mem_base = (uint32_t)&__usr__mem__base__;
	uint32_t mem_len = (uint32_t)&__usr__mem__len__;

	if (!mem_region_init((uint8_t *)mem_base, mem_len, sizeof(mem_buffer_t)))
	{
		printf("User memory initialization failed. Base:0x%x, len:0x%x\n", \
			mem_base, mem_len);
		while (1);
	}

	return;
}


bool alloc_mem_buffer(uint8_t *region_start, uint32_t len, mem_buffer_t **buf_ptr)
{
	mem_region_t *region_ptr = (mem_region_t *)region_start;
	mem_buffer_t *buffer_ptr = (mem_buffer_t *)(region_start + sizeof(mem_region_t));
	uint32_t len_left = region_ptr->region_len;
	uint32_t accum_len = 0;
	uint32_t header_len = region_ptr->buffer_header_len;

	MEM_DEBUG_PRINT("Allocating buffer of len:0x%x\n");

        len += 3;
	len &= 0xFFFFFFFC;

	mem_buffer_t *start_ptr = buffer_ptr;
	while (len_left)
	{
		if (!CHECK_MAGIC_NUM(buffer_ptr->magic_start[0])) {
			printf("Buffer corruption while allocating length:0x%x!!\n\n", len);
			return FALSE;
		}


		if (buffer_ptr->buffer_status == MEM_STATUS_FREE)
		{
			// Found the candidate buffer
			accum_len += buffer_ptr->buffer_len;
			if (accum_len >= len) {
				if (accum_len >= (len + header_len))
				{
					MEM_DEBUG_PRINT("Found buffer. Req len:0x%x, actual_len:0x%x\n", len, accum_len);
					// Split buffer
					mem_buffer_t *next_buffer = (mem_buffer_t *)((uint8_t *)start_ptr + header_len + len);

					SET_MAGIC_NUM(next_buffer->magic_start[0]);
					next_buffer->buffer_len = (accum_len - (len + header_len));
					next_buffer->buffer_status = MEM_STATUS_FREE;
				}
				else
				{
					// Cannot split
					len = accum_len;
				}

				start_ptr->buffer_status = MEM_STATUS_OCCUPIED;
				start_ptr->buffer_len = len;
				break;
			}
			else
			{
				accum_len += header_len;	// Pre-add header len of next buffer
			}
		}
		else
		{
			accum_len = 0;
			start_ptr = (mem_buffer_t *)((uint8_t *)buffer_ptr + buffer_ptr->buffer_len + header_len);
		}

		len_left -= (header_len + buffer_ptr->buffer_len);
		buffer_ptr = (mem_buffer_t *)((uint8_t *)buffer_ptr + buffer_ptr->buffer_len + header_len);
	}

	if (len_left)
	{
		// Found buffer
		*buf_ptr = start_ptr;
		MEM_DEBUG_PRINT("Allocated buffer starting at:0x%x of length:0x%x\n", start_ptr, start_ptr->buffer_len);
		return TRUE;
	}
	else
	{
		printf("Memory not found in region\n");
		return FALSE;
	}
}


bool free_mem_buffer(mem_buffer_t *buf_ptr)
{
	if (!CHECK_MAGIC_NUM(buf_ptr->magic_start[0])) {
		printf("Buffer corruption detected while freeing the buffer: 0x%x\n",
		       (uint32_t)buf_ptr + sizeof(mem_buffer_t));
		return FALSE;
	}

	
	MEM_DEBUG_PRINT("Freeing mem buffer at address: 0x%x\n", buf_ptr);

	buf_ptr->buffer_status = MEM_STATUS_FREE;

	return TRUE;
}


void free(void *p)
{
        uint8_t *ptr = (uint8_t *)p;
	free_mem_buffer((mem_buffer_t *)((uint8_t *)ptr - sizeof(mem_buffer_t)));
	return;
}


uint8_t *malloc(uint32_t size)
{
	mem_buffer_t *ptr = NULL;
	uint32_t mem_base = (uint32_t)&__usr__mem__base__;
	// #warning: No mutex protection in malloc by multiple processes!!!

	if (!alloc_mem_buffer((uint8_t *)mem_base, size, &ptr))
	{
		printf("User memory allocation failed for size: 0x%x\n", size);
		return NULL;
	}

	//#warning: Check malloc return value
	return (uint8_t *)ptr + sizeof(mem_buffer_t);
}

uint8_t *calloc(uint32_t num, uint32_t blk_size)
{
    uint8_t *ptr = NULL;
    uint32_t len = 0;

    if (!num || !blk_size) {
        return NULL;
    }

    len = num * blk_size;
    ptr = malloc(len);
    if (!ptr) { return NULL; }

    memset(ptr, 0, len);

    return ptr;
}
        



bool mem_blocks_init(uint8_t *start_ptr, uint32_t block_len, uint32_t num_blocks)
{
	mem_block_t *block_ptr = (mem_block_t *)start_ptr;
	uint32_t i = 0;

	if (block_len < sizeof(mem_block_t) || (block_len & 0x03)) {
		printf("Insufficient/Invalid block size: 0x%x\n", block_len);
		return FALSE;
	}

	for (i = 0; i < num_blocks; i++)
	{
		SET_MAGIC_NUM(block_ptr->magic_start[0]);
		block_ptr->next_free = (mem_block_t *)((uint8_t *)block_ptr + block_len);
		block_ptr->block_index = i;
		block_ptr = (mem_block_t *)((uint8_t *)block_ptr + block_len);
	}

	return TRUE;
}

mem_block_t * alloc_mem_block(uint8_t **freelist_p)
{
	mem_block_t *block_ptr = (mem_block_t *)*freelist_p;
	if (block_ptr)
	{
		if (!CHECK_MAGIC_NUM(block_ptr->magic_start[0])) {
			printf("Block corruption!!\n");
			return NULL;
		}

		*freelist_p = (uint8_t *)block_ptr->next_free;

		MEM_DEBUG_PRINT("Allocated mem block from freelist: 0x%x\n", block_ptr);
	}
	else
	{
		printf("Freelist exhaust\n");
	}

	
	return block_ptr;
}

bool free_mem_block(uint8_t **freelist_p, mem_block_t *block_ptr)
{
	if (!CHECK_MAGIC_NUM(block_ptr->magic_start[0])) {
		printf("Block corruption!!\n");
		return FALSE;
	}

	block_ptr->next_free = (mem_block_t *)*freelist_p;
	*freelist_p = (uint8_t *)block_ptr;
	return TRUE;
}

void memcpy(uint8_t *dst, uint8_t *src, uint32_t size)
{
   uint32_t i = 0;
   for (i = 0; i < size; i++) {
      *dst = *src;
      dst++; src++;
  }

  return;
}


void memset(uint8_t *ptr, uint8_t value, uint32_t size)
{
    uint32_t i = 0;
    for (i = 0; i < size; i++) {
        *ptr = value;
	ptr++;
    }

    return;
}
	
		
	
	
	

	
